/*
 * findSquares.cpp
 *
 *  Created on: Nov 8, 2012
 *      Author: Matthijs
 */
#include <camera/camera.h>

using namespace cv;

void Camera::findSquares (Mat src)
{
	findSquares(src, 0.0, false);
}

/**
 *  Find squares in the given image frame
 *
 *  This basically proceeds according to certain fixed steps:
 *
 *  	1.	Prepare: remove everything that is not "red" enough.
 *  	2. 	Augment: dilute what's left into complete structures as much  as possible
 *  	3.	Detect:  find separate structures, determine their extents.
 *  	4.  Check: 	 for each structure, check whether it is square and big enough. Stop if so.
 */
void Camera::findSquares (Mat src, float angle, bool identify)
{
//	ROS_INFO("Finding squares.");

	if (!src.rows)
		return;

	int thresh 			= 100;	// Canny edge detector threshold.
	int offset_top 		= 0;	// Ignore this amount of pixels in the top of the image.
	int offset_left 	= 0;	// Ignore this many pixels on the left side of the image.
	RNG rng(12345);				// Random number generator for displaying contours.

	/// Convert image to HSV and blur it
	//cvtColor( src, src, CV_BGR2HSV );
	int erosion_size = 2;
	Mat element = getStructuringElement( MORPH_ELLIPSE,
	                                       Size( 2*erosion_size + 1, 2*erosion_size+1 ),
	                                       Point( erosion_size, erosion_size ) );
	Mat threshold = Mat(src, Rect(offset_left, offset_top,
								  src.cols - (2 * offset_left),
								  src.rows - (2 * offset_top)));

	// Step 1: Blur the image
	blur( threshold, threshold, Size(3,3) );

	// Step 2: remove all pixels not "red" enough
	inRange(threshold, Scalar(0, 0, 80), Scalar(40, 40, 255), threshold);

	// Step 3: dilate the image, so we get more edges connecting.
	dilate(threshold, threshold, element);

//	imshow( "camera window", threshold );

	Mat canny_output;
	vector<vector<Point> > contours;

	/// Detect edges using canny
	Canny( threshold, canny_output, thresh, thresh*4, 3 );

	/// Find contours
	findContours( canny_output, contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);

	if (contours.size() == 0)
		return;

	// Discard all contours not of a certain size and shape.
	RotatedRect box;
	float 		dist;

	// For all contours, determine their size and "squareness":
	for( uint i = 0; i < contours.size(); i++ )
	{
		box 	= minAreaRect(contours[i]);

		float variance = 1.0f - (box.size.width  / box.size.height);

		// Is the box much wider than it is heigh? Not a square!
		if (variance > 0.2 || variance < -0.2 || box.size.height < (0.05 * src.rows))
			continue;

//		ROS_INFO("Contour size, variance: %f, %f, %f ", box.size.width, box.size.height, variance );

		if (box.size.height < 30)
			dist = 0.70;
		else if (box.size.height < 40)
			dist = 0.50;
		else if (box.size.height < 70)
			dist = 0.30;
		else if (box.size.height < 80)
			dist = 0.20;
		else
			dist = 0.10;

		// Find tag
		Rect rectBox = boundingRect(contours[i]);
		rectBox.x  += offset_left;
		rectBox.y  += offset_top;
		Mat tag 	= src(rectBox);

		char  tagname[35];
		if (identify)
		{
			findTags(tag);
		} else {
			// -- Publish tag
			tagMessage.angle   = angle;
			tagMessage.dist    = dist;
			tagMessage.tagId   = -1;
			tagMessage.colorId = -1;
			tag_publisher.publish(tagMessage);
			sprintf(tagname, "snapshots/squares/tag_%d.jpg", imageIndex++);
			imwrite(tagname, tag);
		}
	}
	return;

	 /// Draw contours
	 Mat drawing = Mat::zeros( canny_output.size(), CV_8UC3 );
	 for( uint i = 0; i< contours.size(); i++ )
	 {
		 Scalar color = Scalar( rng.uniform(0, 255), rng.uniform(0,255), rng.uniform(0,255) );
	     drawContours( drawing, contours, i, color, 2, 8);
	 }
	 /// Show in a window
	 imshow( "camera window", drawing );
}



